package org.team751;


import edu.wpi.first.wpilibj.IterativeRobot;
import org.team751.sensors.Ultrasonic;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.team751.commands.Autonomous;
import org.team751.commands.CommandBase;
import org.team751.communication.PandaboardInterface;
import org.team751.util.Utilities;
import org.team751.util.logging.LogLevel;
import org.team751.util.logging.Logger;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot2012 extends IterativeRobot {

    CommandGroup autonomous = new Autonomous();
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {

        // Initialize all subsystems
        CommandBase.init();
        Utilities.printStartupMessage();

    }

    public void disabledContinuous() {

    }
    public void disabledInit() {
        Logger.getInstance().log("Robot disabled.", LogLevel.kStatus);
    }
    public void disabledPeriodic() {
		Logger.getInstance().log("Shooter rate: "+CommandBase.shooterWheels.getActualRate()+" degrees/second", LogLevel.kDebug);
    }

    public void autonomousInit() {
        Logger.getInstance().log("Autonomous mode starting.", LogLevel.kStatus);
        autonomous.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
        Logger.getInstance().log("Teleoperated mode starting.", LogLevel.kStatus);
        Utilities.printAllianceStatusMessage();
        autonomous.cancel();//Ensure that autonomous does not continue
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();

		Logger.getInstance().log("Shooter rate: "+CommandBase.shooterWheels.getActualRate()+" degrees/second", LogLevel.kDebug);
    }
}
